package edu.mapi.ir.controllers;

import edu.mapi.ir.state.EstimatedState;
/**
 * @author ZP
 */
public class RotateController extends AbstractController {

	double targetAngleRads = 0;
	double maxPower = 0.05;
	double powOut = 0;
	
	public RotateController(double targetAngleDegs) {
		
		//while (targetAngleDegs > Math.PI * 2)
		//	targetAngleDegs -= Math.PI * 2;
		//while (targetAngleDegs < -Math.PI * 2)
		//	targetAngleDegs += Math.PI * 2;
		
		this.targetAngleRads = Math.toRadians(targetAngleDegs);
		
		//System.out.println("Rotate to "+Math.toDegrees(targetAngleDegs));
	}
	
	public Actuation decide(Actuation lastActuation, EstimatedState state) {
		double curAngle = state.getRotation();
		double angleDiff = targetAngleRads - curAngle;
		
		if (angleDiff > Math.PI)
			angleDiff -= Math.PI * 2;
		
		if (angleDiff < -Math.PI)
			angleDiff += Math.PI * 2;
		
		double signal = Math.signum(angleDiff);
				
		if (Math.abs(angleDiff) < maxPower * 2) {
			setComplete(true);
			double act = Math.abs(angleDiff) - Math.abs(state.getLOutPow());
			return new Actuation(-signal * act, signal * act);			
		}
		else {
			return new Actuation(-signal * maxPower, signal * maxPower);
		}
	}
}
